/*
 * SmartCANJaguar.cpp
 *
 *  Created on: Apr 1, 2013
 *      Author: trwulff
 */
#ifndef SmartCANJaguar_CPP
#define SmartCANJaguar_CPP
#include "SmartCANJaguar.hpp"

/*
 * Default constructor
 */
SmartCANJaguar::SmartCANJaguar(UINT8 deviceNumber, ControlMode controlMode)
:CANJaguar(deviceNumber,controlMode)
{
	File_Base="SmartCANJaguar";
	DeviceNumber=deviceNumber;
	SmartDashboard::init();
}

/*
 * Default destructor
 */
SmartCANJaguar::~SmartCANJaguar()
{
	
}

void SmartCANJaguar::Calibrate()
{
	
}

void SmartCANJaguar::Status()
{
	SmartDashboard::PutNumber("Set Value"+ConvertItoA(DeviceNumber),this->Get());
	switch (CANJaguar::GetSpeedReference())
	{
	case CANJaguar::kSpeedRef_None:
		SmartDashboard::PutString("Speed Reference"+ConvertItoA(DeviceNumber),"<none>");
		break;
	case CANJaguar::kSpeedRef_Encoder:
		SmartDashboard::PutString("Speed Reference"+ConvertItoA(DeviceNumber),"Encoder");
		break;
	case CANJaguar::kSpeedRef_InvEncoder:
		SmartDashboard::PutString("Speed Reference"+ConvertItoA(DeviceNumber),"InvEncoder");
		break;
	case CANJaguar::kSpeedRef_QuadEncoder:
		SmartDashboard::PutString("Speed Reference"+ConvertItoA(DeviceNumber),"Encoder");
		break;
	}
	
	switch (CANJaguar::GetPositionReference ())
	{
	case CANJaguar::kPosRef_None:
		SmartDashboard::PutString("Position Reference"+ConvertItoA(DeviceNumber),"<none>");
		break;
	case CANJaguar::kPosRef_Potentiometer:
		SmartDashboard::PutString("Position Reference"+ConvertItoA(DeviceNumber),"Potentiometer");
		break;
	case CANJaguar::kPosRef_QuadEncoder:
		SmartDashboard::PutString("Position Reference"+ConvertItoA(DeviceNumber),"Quad Encoder");
		break;
	}
}
string SmartCANJaguar::ConvertItoA(UINT8 deviceNumber)
{
	string ReturnString="";
	int temp1=deviceNumber,temp2;
	if(temp1==0)return "0";
	while(temp1!=0)
	{
		temp2=temp1-(temp1/10)*10;
		
		switch (temp2)
		{
		case 0:
			ReturnString="0"+ReturnString;
			break;

		case 1:
			ReturnString="1"+ReturnString;
			break;
			
		case 2:
			ReturnString="2"+ReturnString;
			break;
			
		case 3:
			ReturnString="3"+ReturnString;
			break;
			
		case 4:
			ReturnString="4"+ReturnString;
			break;

		case 5:
			ReturnString="5"+ReturnString;
			break;

		case 6:
			ReturnString="6"+ReturnString;
			break;

		case 7:
			ReturnString="7"+ReturnString;
			break;

		case 8:
			ReturnString="8"+ReturnString;
			break;

		case 9:
			ReturnString="9"+ReturnString;
			break;
		

		}
		temp1=temp1/10;

		
	}
			return ReturnString;
}

#endif
